//
// Created by zhangzhiwen on 25-5-23.
//

#include "motor.h"
#include "tim.h"
#include "encoder.h"

#define MOTOR_MAX_SPEED 1000

uint16_t last_step = 0;

void motor_init()
{
    HAL_TIM_Base_Start(&htim2);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
}

void motor_set_speed(int32_t speed1, int32_t speed2)
{
    if (speed1 > 0)
    {
        speed1 = speed1>MOTOR_MAX_SPEED?MOTOR_MAX_SPEED:speed1;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, speed1);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 0);
    }
    else if (speed1 < 0)
    {
        speed1 = -speed1>MOTOR_MAX_SPEED?MOTOR_MAX_SPEED:-speed1;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 0);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, speed1);
    }
    else
    {
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1000);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_2, 1000);
    }

    if (speed2 > 0)
    {
        speed2 = speed2>MOTOR_MAX_SPEED?MOTOR_MAX_SPEED:speed2;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, speed2);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 0);
    }
    else if (speed2 < 0)
    {
        speed2 = -speed2>MOTOR_MAX_SPEED?MOTOR_MAX_SPEED:-speed2;
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 0);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, speed2);
    }
    else
    {
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_3, 1000);
        __HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_4, 1000);
    }
}

void motor_move(uint16_t step)
{
    if (step > last_step)
    {
        motor_set_speed(1000, 1000);
        while (step > (encoder_get(&htim3)+encoder_get(&htim4)))
        {
            int16_t error = (int16_t)(encoder_get(&htim3) - encoder_get(&htim4))*10;
            motor_set_speed(900+error, 900-error);
        }
        motor_set_speed(0, 0);
        last_step = step;
    }
    else if (step < last_step)
    {
        motor_set_speed(-1000, -1000);
        while (step < (encoder_get(&htim3)+encoder_get(&htim4)))
        {
            int16_t error = (int16_t)(encoder_get(&htim3) - encoder_get(&htim4));
            motor_set_speed(-900+error, -900-error);
        }
        motor_set_speed(0, 0);
        last_step = step;
    }
}

void motor_reinit()
{
    __HAL_TIM_SET_COUNTER(&htim3, 0);
    __HAL_TIM_SET_COUNTER(&htim4, 0);
    last_step = 0;
}

